import numpy as np

'''将轨迹文件中的四元数转化为外参矩阵'''


def quaternion_to_rotation_matrix(q):
    x, y, z, w, = q
    R = np.array([
        [1 - 2 * y ** 2 - 2 * z ** 2, 2 * x * y + 2 * z * w, 2 * x * z - 2 * y * w],
        [2 * x * y - 2 * z * w, 1 - 2 * x ** 2 - 2 * z ** 2, 2 * y * z + 2 * x * w],
        [2 * x * z + 2 * y * w, 2 * y * z - 2 * x * w, 1 - 2 * x ** 2 - 2 * y ** 2]
    ])
    return R


# 输入和输出文件路径
input_file = 'CameraTrajectory_raw.txt'
output_file = 'CameraTrajectory.txt'

# 读取输入文件
with open(input_file, 'r') as file:
    lines = file.readlines()

# 处理并转换数据
with open(output_file, 'w') as file:
    for line in lines:
        data = list(map(float, line.strip().split()))
        timestamp, tx, ty, tz, qx, qy, qz, qw, = data
        R = quaternion_to_rotation_matrix([qx, qy, qz, qw, ])
        t = np.array([tx, ty, tz]).reshape(3, 1)
        Rt = np.hstack((R, t)).flatten()
        file.write(' '.join(map(str, Rt)) + '\n')
